import json
import re
import socket
import threading
from dronekit import connect, VehicleMode
import time
from pymavlink import mavutil
import math

# 改为当前连接的飞控的端口
connection_string = '10.42.0.10:14550'
print('Connecting to vehicle on: %s' % connection_string)
# connect函数将会返回一个Vehicle类型的对象，即此处的vehicle
# 即可认为是无人机的主体，通过vehicle对象，我们可以直接控制无人机
vehicle = connect('/dev/ttyUSB0', wait_ready=True, baud=921600)


# 高度以米为单位
def arm_and_takeoff(aTargetAltitude):
    """
    Arms vehicle and fly to aTargetAltitude.
    """
    #  进行起飞前检查
    print("Basic pre-arm checks")

    # 解锁无人机（电机将开始旋转）
    print("Arming motors")
    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机）
    vehicle.mode = VehicleMode("GUIDED")
    # 通过设置vehicle.armed状态变量为True，解锁无人机
    vehicle.armed = True

    # 在无人机起飞之前，确认电机已经解锁
    while not vehicle.armed:
        print(" Waiting for arming...")
        time.sleep(1)

    # 发送起飞指令
    print("Taking off!")
    # simple_takeoff将发送指令，使无人机起飞并上升到目标高度
    # send_takeoff_command(aTargetAltitude)
    vehicle.simple_takeoff(aTargetAltitude)
    # # 在无人机上升到目标高度之前，阻塞程序
    while True:
        print(" Altitude: ", vehicle.location.global_relative_frame.alt)
        # 当高度上升到目标高度的0.95倍时，即认为达到了目标高度，退出循环
        # vehicle.location.global_relative_frame.alt为相对于home点的高度
        if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
            print("Reached target altitude")
            break
            # 等待1s
        time.sleep(1)


# 降落
def land():
    vehicle.mode = VehicleMode("LAND")


# 飞行方向速度 * 时间 = 距离
# x表示前后方向,y表示左右方向,z表示垂直高度方向
def send_nav_velocity(velocity_x, velocity_y, velocity_z, duration):
    # 生成SET_POSITION_TARGET_LOCAL_NED命令
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,  # time_boot_ms (not used)
        0, 0,  # target system, target component
        mavutil.mavlink.MAV_FRAME_BODY_NED,  # frame
        0b0000111111000111,  # type_mask (only speeds enabled)
        0, 0, 0,  # x, y, z positions (not used)
        velocity_x, velocity_y, velocity_z,  # x, y, z velocity in m/s
        0, 0, 0,  # x, y, z acceleration (not used)
        0, 0)  # yaw, yaw_rate (not used)
    for x in range(0, duration):
        vehicle.send_mavlink(msg)
        time.sleep(1)


def condition_yaw(heading, relative=True, clock_wise=True):
    """
    设置无人机的朝向角。

    :param heading: 期望的航向角度。
    :param relative: 布尔值，决定是使用相对角度还是绝对角度。
    :param clock_wise: 布尔值，决定转动方向是顺时针还是逆时针。
    """
    isRelative = 1 if relative else 0

    # 修正direction的设置，即使不使用相对角度，也需要指定转动方向
    direction = 1 if clock_wise else -1

    msg = vehicle.message_factory.command_long_encode(
        0, 0,
        mavutil.mavlink.MAV_CMD_CONDITION_YAW,
        0,
        heading,
        0,  # 如果需要，可以设置期望的航向角速度
        direction,
        isRelative,
        0, 0, 0
    )

    vehicle.send_mavlink(msg)
    vehicle.flush()

    # 增加等待无人机完成转向的逻辑（示例）
    if relative:
        # 如果是相对角度，可能需要等待一段时间
        time.sleep(4)  # 默认等待4秒
    else:
        # 如果是绝对角度，可能需要更复杂的逻辑来监控当前位置并等待
        # 这里需要添加检查无人机当前朝向的代码
        pass


def fly_circle(radius):
    """
    让无人机飞一个完整的圆形轨迹。

    :param radius: 圆的半径（单位：米）。
    """
    # 确保半径不超过1.5米
    if radius > 1.5:
        radius = 1.5
        print("Radius limited to 1.5 meters.")
    # 设置飞行速度为0.3米/秒
    speed = 0.3
    # 圆周速度和角速度的关系
    angular_velocity = speed / radius
    # 计算圆周长
    circumference = 2 * math.pi * radius
    # 计算飞行一个完整圆形所需的时间
    duration = circumference / speed
    # 速度分量
    velocity_z = 0  # 假设保持当前高度不变
    # 计算时间步长
    time_step = 1  # 每隔1秒更新一次速度指令
    # 开始飞行圆形轨迹
    start_time = time.time()
    while time.time() - start_time < duration:
        # 计算当前角度
        current_angle = angular_velocity * (time.time() - start_time)
        # 计算速度分量
        velocity_x = speed * math.cos(current_angle)
        velocity_y = speed * math.sin(current_angle)
        # 发送速度指令
        send_nav_velocity(velocity_x, -velocity_y, velocity_z, time_step)
        # # 等待下一个时间步长
        time.sleep(0.1)
    print("圆形执行完成")


def toLeftSingle():
    print("往左一格")
    send_nav_velocity(0, -0.3, 0, 2)
    send_nav_velocity(0, 0, 0, 2)


def toRightSingle():
    print("往右一格")
    send_nav_velocity(0, 0.3, 0, 2)
    send_nav_velocity(0, 0, 0, 2)


def toForwardSingle():
    print("往前一格")
    send_nav_velocity(0.3, 0, 0, 2)
    send_nav_velocity(0, 0, 0, 1)


def toBackSingle():
    print("往后一格")
    send_nav_velocity(-0.3, 0, 0, 2)
    send_nav_velocity(0, 0, 0, 1)


def avoidObstacle():
    global nowX
    global nowY
    global isFirst
    # 根据避障方向执行避障动作
    if direction == 1 and isFirst:
        isFirst = False  # 确保避障只执行一次
        print("执行避障：向左")
        sendNowPosition(0, -1)
        toLeftSingle()
        # 左边补偿
        send_nav_velocity(0, -0.1, 0, 2)
        send_nav_velocity(0, 0, 0, 2)
        nowY -= 1
        sendNowPosition(-3, 0)
        toForwardSingle()
        toForwardSingle()
        toForwardSingle()
        nowX -= 3
        print("避障结束后 (x, y)：" + str(nowX) + "," + str(nowY))


isExist = False  # 是否存在障碍物
direction = 0  # 穿过障碍物的方向
nowX = 7  # 运动过程中的实时位置X
nowY = 6  # 运动过程中的实时位置Y
isFirst = True  # 避免避障指令输入
headToward = 1  # 飞机头朝向 1 朝前  -1 朝后
first_height = 0  # 初始高度


# 启用线程一直接收 是否有避障指令
def isExistObstacle():
    global isExist
    global direction
    global isFirst
    isExist = False
    # 创建UDP套接字并绑定到指定的端口
    udp_socket1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定UDP套接字到 1000
    udp_socket1.bind(('10.42.0.10', 10000))
    print(f"UDP server listening on 接收避障指令{('10.42.0.10', 10000)}")
    while True:
        # 接收数据
        data, addr = udp_socket1.recvfrom(1024)
        data_length = len(data)
        print(f"Received data: {data} 长度为:{data_length}")
        command = data.decode('utf-8')
        print(command)
        if command == "左避障" and isFirst:
            isExist = True
            direction = 1
            print("前方障碍物 左避障")
            avoidObstacle()  # 测试
        if command == "1":  # 强制中断
            land()


# 定义一个函数来发送上次终止航线任务的位置给web中心
def sendNowPosition(x, y):
    UDP_IP_ADDRESS = '10.42.0.226'
    UDP_PORT_NO = 20000
    # 创建UDP客户端套接字
    clientSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    latsPosition = {'nowX': x, 'nowY': y}
    print(latsPosition)
    # 发送当前位置到UDP目标电脑
    clientSock.sendto(json.dumps(latsPosition).encode('utf-8'), (UDP_IP_ADDRESS, UDP_PORT_NO))


# 定义一个函数来处理接收到的UDP数据
def handle_udp_data():
    # 创建UDP套接字并绑定到指定的端口
    udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定UDP套接字到指定的端口
    udp_socket.bind(('10.42.0.10', 1111))
    print(f"UDP server listening on {('10.42.0.10', 1111)}")
    while True:
        # 接收数据
        data, addr = udp_socket.recvfrom(1024)
        data_length = len(data)
        print(f"Received data: {data} 长度为:{data_length}")
        if data_length > 1:
            # 尝试将数据解析为JSON
            point_data = json.loads(data.decode('utf-8'))
            execute_instructions(point_data)
        else:  # 起飞降落进 1 2
            numbers = re.findall(r'\d+', data.decode('utf-8'))
            if numbers:
                print("数字")
                execute_instructions(data.decode('utf-8'))


# 执行航点任务
def executePoints(flight_direction):
    global isExist, headToward
    global nowX  # 穿过障碍物后 下次航点开始的X
    global nowY  # 穿过障碍物后 下次航点开始的Y
    # 航点的移动轨迹 先左右 后前后
    for f_direction in flight_direction:
        x = f_direction['x']
        y = f_direction['y']
        for i in range(0, abs(y)):  # 左右移动
            if headToward == 1:  # 机头朝前
                if y < 0:
                    toLeftSingle()
                    nowY -= 1
                    # 发送当前移动到web
                    sendNowPosition(0, -1)
                else:
                    toRightSingle()
                    nowY += 1
                    # 发送当前移动到web
                    sendNowPosition(0, 1)
            else:  # 机头朝后
                if y < 0:
                    toRightSingle()
                    nowY -= 1
                    # 发送当前移动到web
                    sendNowPosition(0, -1)
                else:
                    toLeftSingle()
                    nowY += 1
                    # 发送当前移动到web
                    sendNowPosition(0, 1)
        for i in range(0, abs(x)):  # 前后移动
            # 识别前方障碍物
            if isExist:  # 检查是否有避障指令发送过来
                avoidObstacle()
                break
            else:
                if x > 0 and headToward == 1:  # 机头朝后
                    headToward = -1
                    print("调转机头180")
                    condition_yaw(180, True, True)
                    time.sleep(4)
                if headToward == 1:
                    toForwardSingle()
                    # 发送当前移动到web
                    sendNowPosition(-1, 0)
                    # 更新当前位置
                    nowX -= 1
                if headToward == -1:  # 机头朝后 先转头
                    toForwardSingle()
                    # 发送当前移动到web
                    sendNowPosition(1, 0)
                    # 更新当前位置
                    nowX += 1
                time.sleep(1)  # 等待1秒检测前方障碍物
        if isExist:
            isExist = False
            print("中断本次航点任务")
            break  # 结束本次航点任务


# 飞往目标点
def goToTarget(targetX, targetY):
    """
        从当前位置(nowX, nowY)移动到目标位置(targetX, targetY)。
        参数:
        targetX (int): 目标位置的x坐标。
        targetY (int): 目标位置的y坐标。
        全局变量:
        nowX (int): 当前位置的x坐标。
        nowY (int): 当前位置的y坐标。
    """
    global nowX, nowY
    diffX = targetX - nowX
    diffY = targetY - nowY
    print("去往指定航点时 (nowX,nowY)：" + str(nowX) + "," + str(nowY))
    for i in range(0, abs(diffY)):  # 左右移动
        # 考虑 机头朝前 还是朝后
        if headToward == 1:  # 机头朝前
            if diffY < 0:  # 左
                toLeftSingle()
                nowY -= 1
                sendNowPosition(0, -1)
            else:
                toRightSingle()
                nowY += 1
                sendNowPosition(0, 1)
        if headToward == -1:
            if diffY < 0:
                toRightSingle()
                nowY -= 1
                # 发送当前移动到web
                sendNowPosition(0, -1)
            else:
                toLeftSingle()
                nowY += 1
                # 发送当前移动到web
                sendNowPosition(0, 1)
    for i in range(0, abs(diffX)):  # 前后移动
        # 往前移动
        if diffX < 0 and headToward == 1:  # 朝前
            toForwardSingle()
            # 更新当前位置
            nowX -= 1
            sendNowPosition(-1, 0)
        if diffX > 0 and headToward == -1:  # 机头朝后 同时往前
            toForwardSingle()
            # 更新当前位置
            nowX += 1
            sendNowPosition(1, 0)


# 定义一个函数来按照队列顺序执行动作
# 1 起飞  2  降落
def execute_instructions(instruction):
    global first_height
    flight_direction = []  # 飞行方向
    # 检查指令是否为字典类型 读入航点数据
    # 执行固定航点任务
    if len(instruction) > 2:
        print("执行航点任务")
        print(instruction)
        # 更新起点位置
        centerX = nowX
        centerY = nowY
        print("开始执行航点任务 当前无人机位置（x,y）: " + str(nowX) + "," + str(nowY))
        # 计算航点与无人机当前位置的差值
        for point in instruction:
            pointX = point['x']
            pointY = point['y']
            pointH = point['h']
            diffX = pointX - centerX
            diffY = pointY - centerY
            diffH = pointH - 0.6
            # diffx diffy 为要飞的格子数
            # -x 为左边 -y 为前边
            # gridX gridY 存放本次方向执行开始对应的网格位置 方便无人机中断后新指令的到来
            position = {
                'x': diffX,
                'y': diffY,
                'h': diffH,
                'gridX': centerX,
                'gridY': centerY
            }
            flight_direction.append(position)
            # 更新当前位置为下一个点
            centerX = pointX
            centerY = pointY
        executePoints(flight_direction)
    # 执行指令对应的动作
    if instruction == "1":
        # 默认飞行高度0.6m
        first_height = vehicle.location.global_relative_frame.alt
        print(first_height)
        if -0.01 < first_height < 0.09:  # 高度在合理范围内
            arm_and_takeoff(0.2)
            print(f"Executing instruction: 起飞")
        else:
            print("不在起飞范围内")
    if instruction == "2":
        land()
        print(f"Executing instruction: 降落")
    if instruction == "3":
        print(f"Executing instruction: 往前一格")
        toForwardSingle()
    if instruction == "4":
        toBackSingle()
        print(f"Executing instruction: 往后一格")
    if instruction == "5":
        toLeftSingle()
        print(f"Executing instruction: 往左一格")
    if instruction == "6":
        toRightSingle()
        print(f"Executing instruction: 往右一格")
    if instruction == "7":
        fly_circle(0.7)
        print(f"Executing instruction: 圆形轨迹 指令之间等待时间0")
    if instruction == "8":
        print("返航")
        goToTarget(6, 6)
        print("到达指定航点 (nowX,nowY)：" + str(nowX) + "," + str(nowY))
    if instruction == "9":
        print("测试调转机头180")
        condition_yaw(180, True, True)
    time.sleep(0.5)


# 创建一个线程来处理UDP数据
udp_thread = threading.Thread(target=handle_udp_data)
# 创建一个线程接受web传来的避障识别指令
avoidObstacle_thread = threading.Thread(target=isExistObstacle)
udp_thread.start()
avoidObstacle_thread.start()
